import sys
import threading
import time
import rclpy
from rclpy.executors import ExternalShutdownException
from semantic_nav_interfaces.action import NavStr
from semantic_nav.action_utils import PriorQueClient


def main(args=None):
    rclpy.init(args=args)
    try:
        semantic_nav_client = PriorQueClient(action_type=NavStr, action_name="semantic_nav")
        goal0 = NavStr.Goal()
        goal0.priority = goal0.HIGH_PRIORITY
        goal0.type = goal0.TYPE_QA
        goal0.input = "test goal0"
        goal1 = NavStr.Goal()
        goal1.priority = goal1.MID_PRIORITY
        goal1.type = goal1.TYPE_DECISION
        goal1.input = "test goal1"
        goal2 = NavStr.Goal()
        goal2.priority = goal2.LOW_PRIORITY
        goal2.type = goal2.TYPE_QA
        goal2.input = "test goal2"
        goal3 = NavStr.Goal()
        goal3.priority = goal3.LOW_PRIORITY
        goal3.type = goal3.TYPE_QA
        goal3.input = "test goal3"
        semantic_nav_client.send_goals([goal0, goal1, goal2, goal3])
        thread = threading.Thread(target=rclpy.spin, args=(semantic_nav_client,))
        thread.start()
        time.sleep(1)
        semantic_nav_client.send_goals([goal1, goal2, goal1])
        # time.sleep(3)
        # semantic_nav_client.cancel_goal()
        # time.sleep(3)
        # semantic_nav_client.cancel_goals()
        thread.join()
    except KeyboardInterrupt:
        pass
    except ExternalShutdownException:
        sys.exit(1)


if __name__ == '__main__':
    main()
